#include <iostream>
#include <string>
#include <vector>
#include <cstdio>
#include <cmath>
using namespace std;

#include "Control.h"
CControl::CControl()
{
	this->_destPrev.x = 0;
	this->_destPrev.y = 0;
	this->_destPrev.z = 0;
	this->_statePrev.x = 1;
	this->_statePrev.y = 0;
	this->_statePrev.z = 0;
}

CControl::~CControl()
{
}
bool CControl::OnNewMail(MOOSMSG_LIST &NewMail)
{
	MOOSMSG_LIST::iterator iterMSG;
	for(iterMSG = NewMail.begin(); iterMSG != NewMail.end(); iterMSG++){
		CMOOSMsg & rMsg = *iterMSG;
		if(MOOSStrCmp(rMsg.GetKey(),"State"))
		{
	 		this->OnState(rMsg);
		}
		if(MOOSStrCmp(rMsg.GetKey(),"Destination"))
		{
	 		this->OnDestination(rMsg);
		}

	}

	return true;
}

bool CControl::OnConnectToServer()
{
		this->DoRegistrations();
        return true;
}
int br = 50;
bool CControl::Iterate()
{
	br--;
	if(br == 0){
	cout << "PAUZA" << endl;
	//cin >> br;
	}
/*	string strControl;
	strControl = MOOSFormat("ControlCount=%d,control1=%f,control2=%f,control3=%f",
		3,
		-5.0,
		5.0,
		0.0);
	this->m_Comms.Notify("Control", strControl, MOOSTime());*/
        return true;
}
bool CControl::OnStartUp()
{
		this->DoRegistrations();
        return true;
}

void CControl::DoRegistrations(){
	this->m_Comms.Register("State", 0);
	this->m_Comms.Register("Destination", 0);
	return;
}
bool inCourseLimits(double course){
	if(course < 0){
		course = course *-1;
	}
	return course <= 0.000001;
}
bool inDistanceLimits(double distance){
	if(distance < 0){
		distance = distance*-1;
	}
	return distance <= 0.001;
}
bool inDiveLimits(double dive){
	if(dive < 0){
		dive = dive * -1;
	}
	return dive <= 0.001;
}

void CalcCDD(
	double xdest,double ydest, double zdest,
	double xstate,double ystate, double zstate,
	double psistate,
	double &course, double &dive, double &distance,
	double &xdistance, double &ydistance){

	double angle;
	xdistance = xdest - xstate;
	ydistance = ydest - ystate;
	if((xdest - xstate) == 0){
		if(ydest > ystate)
			angle = PI/2;
		else if(ydest < ystate)
			angle = -PI/2;
		else
			angle = 0;
	}else{
		angle = atan(
			(ydest - ystate)
			/
			(xdest - xstate)
			);
		if((xdest - xstate) < 0)
			angle += PI;
	}

	distance = sqrt(
		pow(
			ydest - ystate,
			2)
			+
		pow(
			xdest - xstate,
			2)
			);

	dive = zdest - zstate;
	double psi = psistate - (int)(psistate/(2*PI))*2*PI;
	course = (angle - psi) - (int)((angle - psi)/(2*PI))*2*PI;
}
//Propulsion const
double const Cthf = 1.5e-4;
double const Cthb = 1.5e-4;
double const Ctvf = 1.5e-4;
double const Ctvb = 1.5e-4;
//Rov const
double	Ix = 2.28e-2; //25
double	Iy = 2.39e-2; //50
double	Iz = 2.53e-2; //50
double	Ixy = 0;
double	Iyz = 0;
double	Ixz = 0;

double	m = 50;
double	xG = 0;
double	yG = 0;
double	zG = 0;

double	xB = 0;
double	yB = 0;
double	zB = -0.1;

double	Xu = 2.3;
double	Yv = 8.01;
double	Zw = 5.81;

double	Kp = 0.0009;
double	Mq = 0.0012;
double	Nr = 0.0048;

double	Xuu = 8.28;
double	Yvv = 23.69;
double	Zww = 20.52;

double	Kpp = 0.0048;
double	Mqq = 0.0069;
double	Nrr = 0.0089;

double	Xud = 1.94;
double	Yvd = 6.05;
double	Zwd = 3.95;
double	Kpd = 3.26e-2;
double	Mqd = 1.75e-2;
double	Nrd = 3.21e-2;

double	kL = 0.0005;
double	kD = 0.0005;
double	kY = 0.0015;
double	kl = -0.0001;
double	km = -0.0002;
double	kn = 0.001;

double	Clp = -0.1;
double	Clr = -0.1;
double	Cmq = -2;
double	Cma = -1;

double	rho = 1.025;
double	A = 0.135;//1
double	l = 0.36;
double	h = 0.23;
double	W = 500;
double	B = 500;

double const R = 0.175;
void LjapunovCourse(
	double course, double psistate, double rstate,
	double &speed_le, double &speed_ri)
{
	double V = -2*rstate*(Iz+Nrd+Nrr*abs(rstate))+Nr*rstate;
	double K = abs(V) + 0.01;
	double s = (rstate + -2*course);
	double Fs;
	if(s > 0.02) Fs = 1;
	else if(s < -0.02) Fs = -1;
	else Fs = s/0.02;
	double N = V - 0.5*s - K*Fs;

	if(N > 0){
		speed_ri = sqrt(N/(2*R*Cthb));
		speed_le = -speed_ri;
	}else{
		speed_ri = -sqrt(-N/(2*R*Cthb));
		speed_le = -speed_ri;
	}
}

void LjapunovDive(
		double dive,
		double zstate,
		double wstate,
		double &speed_vert)
{
	double V = -4*wstate*(m+Zwd+abs(wstate))+Zw*wstate;
	double K = abs(V) + 0.01;
	double s = (wstate + -2*dive);
	double Fs;
	if(s > 0.01) Fs = 1;
	else if(s < -0.01) Fs = -1;
	else Fs = s/0.01;
	double Z = V - 0.5*s - K*Fs;

	if(Z > 0){
		speed_vert = sqrt(Z/Ctvf);
	}else{
		speed_vert = -sqrt(-Z/Ctvf);
	}


}


void LjapunovForwardX(
	double xdistance,
	double xstate,
	double ustate,
	double &speed_le,
	double &speed_ri)
{
	double V = -2*ustate*(m+Xud+Xuu*abs(ustate))+Xu*ustate;
	double K = abs(V) + 0.01;
	double s = ustate - 2*xdistance;
	double Fs;
	if(s > 0.01) Fs = 1;
	else if(s < -0.01) Fs = -1;
	else Fs = s/0.01;
	double X = V - 5*s - K*Fs;

	if(X > 0){
		speed_ri = sqrt(X/(2*Cthb));
		speed_le = speed_ri;
	}else{
		speed_ri = -sqrt(-X/(2*Cthb));
		speed_le = speed_ri;
	}
}

void LjapunovForwardY(
	double ydistance,
	double ystate,
	double vstate,
	double &speed_le,
	double &speed_ri)
{
	double V = -2*vstate*(m+Yvd+Yvv*abs(vstate))+Yv*vstate;
	double K = abs(V) + 0.01;
	double s = vstate - 2*ydistance;
	double Fs;
	if(s > 0.01) Fs = 1;
	else if(s < -0.01) Fs = -1;
	else Fs = s/0.01;
	double Y = V - 5*s - K*Fs;

	if(Y > 0){
		speed_ri = sqrt(Y/(2*Cthb));
		speed_le = speed_ri;
	}else{
		speed_ri = -sqrt(-Y/(2*Cthb));
		speed_le = speed_ri;
	}
}

double H = 5.0;
double K = 5.0;
double C = 250.0;
bool CControl::OnState(CMOOSMsg& msg){

	//msg.Trace(); //print msg in details
	string strState = msg.GetString();
	cout << "RECV: " << strState << endl;

	MOOSValFromString (this->_statePrev.u , strState, "u", false);
	MOOSValFromString (this->_statePrev.v , strState, "v", false);
	MOOSValFromString (this->_statePrev.w , strState, "w", false);
	MOOSValFromString (this->_statePrev.p , strState, "p", false);
	MOOSValFromString (this->_statePrev.q , strState, "q", false);
	MOOSValFromString (this->_statePrev.r , strState, "r", false);
	MOOSValFromString (this->_statePrev.x , strState, "x", false);
	MOOSValFromString (this->_statePrev.y , strState, "y", false);
	MOOSValFromString (this->_statePrev.z , strState, "z", false);
	MOOSValFromString (this->_statePrev.phi , strState, "phi", false);
	MOOSValFromString (this->_statePrev.theta , strState, "theta", false);
	MOOSValFromString (this->_statePrev.psi , strState, "psi", false);

	double course, dive, distance, xdistance, ydistance;
	CalcCDD(
		this->_destPrev.x,
		this->_destPrev.y,
		this->_destPrev.z,
		this->_statePrev.x,
		this->_statePrev.y,
		this->_statePrev.z,
		this->_statePrev.psi,
		course,
		dive,
		distance,
		xdistance,
		ydistance
	);

	cout << "course: " << course <<  endl;
	cout << "dive: " << dive <<  endl;
	cout << "dist: " << distance <<  endl;
	cout << "xdist: " << xdistance <<  endl;
	cout << "ydist: " << ydistance <<  endl;

	double speed_le, speed_ri, speed_vert;
	speed_le = 0;
	speed_ri = 0;
	speed_vert = 0;

	if(inCourseLimits(course)){
		speed_le = 0;
		speed_ri = 0;
	}else
	{
		LjapunovCourse(
			course,
			this->_statePrev.psi,
			this->_statePrev.r,
			speed_le,
			speed_ri);
	}

	/*When the course is set and there is no Z rotation*/
	if(this->_statePrev.r == 0.0){
		if(inDistanceLimits(distance)){
			speed_le = 0;
			speed_ri = 0;
		}else{
			//double distance_prev = sqrt(pow(this->_statePrev.x,	2)+pow(this->_statePrev.y,2));
			//cout << "dist_prev: " << distance_prev << endl;
			LjapunovForwardX(
				xdistance,
				this->_statePrev.x,
				this->_statePrev.u,
				speed_le,
				speed_ri
			);
		}
	}

	/*If the course is wright and the destination xy is reached*/
	if(inDistanceLimits(distance) == true && this->_statePrev.r == 0.0 && this->_statePrev.u == 0.0){
		if(inDiveLimits(dive)){
			speed_vert = 0;
		}else
		{
			LjapunovDive(
				dive,
				this->_statePrev.z,
				this->_statePrev.w,
				speed_vert);
		}
	}

	cout << "speed_le: " << speed_le << endl;
	cout << "speed_ri: " << speed_ri << endl;

	if(speed_le > 100) speed_le = 100;
	if(speed_le < -100) speed_le = -100;
	if(speed_ri > 100) speed_ri = 100;
	if(speed_ri < -100) speed_ri = -100;
	if(speed_vert > 100) speed_vert = 100;
	if(speed_vert < -100) speed_vert = -100;

	string strControl;
	strControl = MOOSFormat("ControlCount=%d,control1=%f,control2=%f,control3=%f",
		3,
		speed_le,
		speed_ri,
		speed_vert);

	cout << "SEND: " << strControl << endl;
	this->m_Comms.Notify("Control", strControl, MOOSTime());

	return true;
}

bool CControl::OnDestination(CMOOSMsg& msg){

	//msg.Trace(); //print msg in details
	string strDest = msg.GetString();
	cout << "RECV: " << strDest << endl;

	MOOSValFromString (this->_destPrev.x , strDest, "x", false);
	MOOSValFromString (this->_destPrev.y , strDest, "y", false);
	MOOSValFromString (this->_destPrev.z , strDest, "z", false);

	double course, dive, distance, xdistance, ydistance;
	CalcCDD(
		this->_destPrev.x,
		this->_destPrev.y,
		this->_destPrev.z,
		this->_statePrev.x,
		this->_statePrev.y,
		this->_statePrev.z,
		this->_statePrev.psi,
		course,
		dive,
		distance,
		xdistance,
		ydistance
	);

	double speed_le, speed_ri, speed_vert;
	speed_le = 0;
	speed_ri = 0;
	speed_vert = 0;

	LjapunovCourse(
		course,
		this->_statePrev.psi,
		this->_statePrev.r,
		speed_le,
		speed_ri);

	if(inCourseLimits(course)){
		speed_le = 0;
		speed_ri = 0;
	}

	if(speed_le > 100) speed_le = 100;
	if(speed_le < -100) speed_le = -100;
	if(speed_ri > 100) speed_ri = 100;
	if(speed_ri < -100) speed_ri = -100;
	if(speed_vert > 100) speed_vert = 100;
	if(speed_vert < -100) speed_vert = -100;

	string strControl;
	strControl = MOOSFormat("ControlCount=%d,control1=%f,control2=%f,control3=%f",
		3,
		speed_le,
		speed_ri,
		speed_vert);

	cout << "SEND: " << strControl << endl;
	this->m_Comms.Notify("Control", strControl, MOOSTime());


	return true;
}


